#pragma once
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <dlib/opencv.h>  
#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
class PointCloudBase
{
#define  INVALID 1e+10
public:
	typedef pcl::PointXYZI PCLPointI;
	typedef pcl::PointCloud<PCLPointI> PointICloudDef;
	typedef PointICloudDef::Ptr PPointICloudDef;
	typedef pcl::search::KdTree<PCLPointI> KdTreeI;
	typedef KdTreeI::Ptr PKdTreeI;


	typedef pcl::PointXYZL PCLPoint;
	typedef pcl::PointCloud<PCLPoint> PointCloudDef;
	typedef PointCloudDef::Ptr PPointCloudDef;
	typedef PointCloudDef::iterator ItePoint;

	typedef pcl::Normal PCLNormal;
	typedef pcl::PointCloud<PCLNormal> NormalCloudDef;
	typedef NormalCloudDef::Ptr PNormalCloudDef;
	typedef NormalCloudDef::iterator IteNormal;


	typedef pcl::PointNormal PCLPointNormal;
	typedef pcl::PointCloud<PCLPointNormal> PointNormalCloudDef;
	typedef PointNormalCloudDef::Ptr PPointNormalCloudDef;
	typedef PointNormalCloudDef::iterator ItePointNormal;

	typedef pcl::search::KdTree<PCLPoint> KdTree;
	typedef KdTree::Ptr PKdTree;

	typedef pcl::octree::OctreePointCloudSearch<PCLPoint> OCTree;
	typedef OCTree::Ptr POCTree;
public:
	PointCloudBase();
	~PointCloudBase();
	PCLPoint Add(const PCLPoint& a, const PCLPoint& b) const;
	PCLPoint Sub(const PCLPoint& a, const PCLPoint& b) const;
	PCLPoint Mul(const PCLPoint& a, const float& b) const;
	PCLPoint PntTransform(const PCLPoint& pnt, const Eigen::Matrix4Xf& matTransform);
	void GetAABBBox(const PPointCloudDef& pSrc, PCLPoint& pntMin, PCLPoint& pntMax);
	float SquardDis(const PCLPoint& p0, const PCLPoint& p1);
	float SquardDis2(const PCLPoint& p0, const PCLPoint& p1);
	PCLPoint ToPCLPoint(float x, float y, float z, int label = 0);

	inline bool IsValidValue(float data) const{
		return fabs(data - INVALID) > 1e-5;
	}
	inline void SetAsInvalidValue(float& data) const { data = INVALID; }
	float AABBBoxOverlap(const PCLPoint& pntMin1, const PCLPoint& pntMax1, const PCLPoint& pntMin2, const PCLPoint& pntMax2) const;
	Eigen::Vector3f ToVector3(const PCLPoint& pnt) const 
	{
		 Eigen::Vector3f ret(pnt.x, pnt.y, pnt.z);
		 return ret;
	}
	PCLPoint ToPCLPoint(const Eigen::Vector3f& pnt, uint32_t iLabel=0) const
	{
		PCLPoint ret;
		ret.x = pnt.x();
		ret.y = pnt.y();
		ret.z = pnt.z();
		ret.label = iLabel;
		return ret;
	}
	float DisToLine(const Eigen::Vector3f& pnt, const Eigen::Vector3f& pntFrom, const Eigen::Vector3f& dir) const 
	{
		Eigen::Vector3f dirLine = dir; dirLine.normalize();
		Eigen::Vector3f ve = pnt - pntFrom;

		Eigen::Vector3f veTemp = ve - ve.dot(dirLine)*dirLine;
		float ret =  veTemp.norm();
		return ret;
	}
	float DisOnLine(const Eigen::Vector3f& pnt, const Eigen::Vector3f& pntFrom, const Eigen::Vector3f& dir) const
	{
		Eigen::Vector3f dirLine = dir; dirLine.normalize();
		Eigen::Vector3f ve = pnt - pntFrom;
		return ve.dot(dirLine);
	}
	PPointNormalCloudDef LoadXYZ(string strFileName);
	pcl::PolygonMesh ToMeshWithNormal(PPointNormalCloudDef pClound);
	pcl::PolygonMesh ToMesh(PPointCloudDef pClound);
	pcl::PolygonMesh RemoveMesh(const pcl::PolygonMesh& mesh);
	void ShowMesh(const pcl::PolygonMesh& mesh);
	PPointCloudDef PairwiseNDT(PPointCloudDef pScr, PPointCloudDef pDes);

	bool PairwiseICP(PPointCloudDef pScr, PPointCloudDef pDes, PPointCloudDef& pRet, Eigen::Matrix4Xf & matTransform);

	PPointCloudDef StatisticalRemoval(PPointCloudDef pCloudDes, int iNumMean = 20, float fVarMul = 1);
	PPointCloudDef BilateralRemoval(PPointCloudDef pCloudDes, float halfSize=5.0f, float stdDev=0.001);
	float GetArea(const vector<PCLPoint>& vPntPolyon);


	void SaveToScr(string strFileName, const PPointCloudDef& pSrc, int inter = 1, float xOffset = 0, float yOffset = 0, float zOffset = 0, float fScale = 1);
	void SaveToTxt(string strFileName, const PPointCloudDef& pSrc, float xOffset = 0, float yOffset = 0, float zOffset = 0, float fScale = 1);
	void SavePointDataToScr(string strFileName, const vector<PCLPoint>& vPnt, float xOffset = 0, float yOffset = 0, float zOffset = 0, float fScale = 1);
	void SaveMatrix(string strFileName, const Eigen::Matrix4Xf & mat);
	void SaveInfoToScr(string strFileName, const PPointCloudDef& pSrc, float xOffset = 0, float yOffset = 0, float zOffset = 0, float fScale = 1);
	PPointCloudDef SimpleCloud(const PPointCloudDef& pSrc);

	void SaveMeshAsBin(string strFileName, const pcl::PolygonMesh& mesh, float xOffset = 0, float yOffset = 0, float zOffset = 0, float fScale = 1000.0);
	void LoadMeshAsBin(string strFileName, pcl::PolygonMesh& mesh);
	void SavePointArrayAsMark(string strFileName, const vector<PCLPoint>& vPnt, float xOffset = 0, float yOffset = 0, float zOffset = 0, float fScale = 1000);
	void SavePointArrayAsMark(string strFileName, PPointCloudDef pClound, float xOffset = 0, float yOffset = 0, float zOffset = 0, float fScale = 1000);

	PPointICloudDef ToPointICloud(PPointCloudDef pClound, int iIntensity = 0)
	{
		PPointICloudDef pCloundXYZI = PPointICloudDef(new PointICloudDef());
		for (PCLPoint pnt : pClound->points)
		{
			PCLPointI pntI;
			pntI.x = pnt.x;
			pntI.y = pnt.y;
			pntI.z = pnt.z; pntI.intensity = iIntensity;
			pCloundXYZI->push_back(pntI);
		}
		return pCloundXYZI;
	}

	PPointCloudDef ToPointCloud(PPointICloudDef pClound, int iIntensity = 0)
	{
		PPointCloudDef pCloundXYZ = PPointCloudDef(new PointCloudDef());
		for (PCLPointI pnt : pClound->points)
		{
			PCLPoint pntI;
			pntI.x = pnt.x;
			pntI.y = pnt.y;
			pntI.z = pnt.z;
			pCloundXYZ->push_back(pntI);
		}
		return pCloundXYZ;
	}
protected:
};

